/*
 ## Cypress FX3 Camera Kit source file (sensor.c)
 ## ===========================
 ##
 ##  Copyright Cypress Semiconductor Corporation, 2010-2012,
 ##  All Rights Reserved
 ##  UNPUBLISHED, LICENSED SOFTWARE.
 ##
 ##  CONFIDENTIAL AND PROPRIETARY INFORMATION
 ##  WHICH IS THE PROPERTY OF CYPRESS.
 ##
 ##  Use of this file is governed
 ##  by the license agreement included in the file
 ##
 ##     <install>/license/license.txt
 ##
 ##  where <install> is the Cypress software
 ##  installation root directory path.
 ##
 ## ===========================
 */

/* This file implements the I2C based driver for an image sensor that uses I2C
 for control in the FX3 HD 720p camera kit.
 */

#include <cyu3system.h>
#include <cyu3os.h>
#include <cyu3dma.h>
#include <cyu3error.h>
#include <cyu3uart.h>
#include <cyu3i2c.h>
#include <cyu3types.h>
#include <cyu3gpio.h>
#include <cyu3utils.h>
#include "sensor.h"


/* This function inserts a delay between successful I2C transfers to prevent
 false errors due to the slave being busy.
 */
static void SensorI2CAccessDelay(CyU3PReturnStatus_t status) {
	/* Add a 10us delay if the I2C operation that preceded this call was successful. */
	if (status == CY_U3P_SUCCESS)
		CyU3PBusyWait(10);
}

/* Write to an I2C slave with two bytes of data. */
CyU3PReturnStatus_t SensorWrite2B(uint8_t slaveAddr, uint8_t highAddr,
		uint8_t lowAddr, uint8_t highData, uint8_t lowData) {
	CyU3PReturnStatus_t apiRetStatus = CY_U3P_SUCCESS;
	CyU3PI2cPreamble_t preamble;
	uint8_t buf[2];

#if 0
	/* Validate the I2C slave address. */
	if ((slaveAddr != SENSOR_ADDR_WR) && (slaveAddr != I2C_MEMORY_ADDR_WR)) {
		CyU3PDebugPrint(4, "I2C Slave address is not valid!\n");
		return 1;
	}
#endif

	/* Set the parameters for the I2C API access and then call the write API. */
	preamble.buffer[0] = slaveAddr;
	preamble.buffer[1] = highAddr;
	preamble.buffer[2] = lowAddr;
	preamble.length = 3; /*  Three byte preamble. */
	preamble.ctrlMask = 0x0000; /*  No additional start and stop bits. */

	buf[0] = highData;
	buf[1] = lowData;

	apiRetStatus = CyU3PI2cTransmitBytes(&preamble, buf, 2, 0);
	SensorI2CAccessDelay(apiRetStatus);

	return apiRetStatus;
}

CyU3PReturnStatus_t SensorWrite(uint8_t slaveAddr, uint8_t highAddr,
		uint8_t lowAddr, uint8_t count, uint8_t *buf) {
	CyU3PReturnStatus_t apiRetStatus = CY_U3P_SUCCESS;
	CyU3PI2cPreamble_t preamble;

#if 0
	/* Validate the I2C slave address. */
	if ((slaveAddr != SENSOR_ADDR_WR) && (slaveAddr != I2C_MEMORY_ADDR_WR)) {
		CyU3PDebugPrint(4, "I2C Slave address is not valid!\n");
		return 1;
	}
#endif

	if (count > 64) {
		CyU3PDebugPrint(4, "ERROR: SensorWrite count > 64\n");
		return 1;
	}

	/* Set up the I2C control parameters and invoke the write API. */
	preamble.buffer[0] = slaveAddr;
	//preamble.buffer[1] = highAddr;
	//preamble.buffer[2] = lowAddr;
	preamble.buffer[1] = lowAddr;
	//preamble.length = 3;
	preamble.length = 2;
	preamble.ctrlMask = 0x0000;

#if 0
        CyU3PDebugPrint(4, "pre0 %x, pre1%x, pre.len %d, pre.msk %x\r\n",
            preamble.buffer[0],
            preamble.buffer[1],
            preamble.length,
            preamble.ctrlMask);
#endif

	apiRetStatus = CyU3PI2cTransmitBytes(&preamble, buf, count, 0);
	
        CyU3PDebugPrint(4, "WR 0x%x, return = %x\r\n", lowAddr, apiRetStatus);
	
	SensorI2CAccessDelay(apiRetStatus);

	return apiRetStatus;
}


CyU3PReturnStatus_t ADV7611Write(uint8_t slaveAddr,
		uint8_t lowAddr, uint8_t val) {
	CyU3PReturnStatus_t apiRetStatus = CY_U3P_SUCCESS;
	CyU3PI2cPreamble_t preamble;

	uint8_t buf = 0;
	buf = val;

#if 0
	/* Validate the I2C slave address. */
	if ((slaveAddr != SENSOR_ADDR_WR) && (slaveAddr != I2C_MEMORY_ADDR_WR)) {
		CyU3PDebugPrint(4, "I2C Slave address is not valid!\n");
		return 1;
	}
#endif

#if 0
	if (count > 64) {
		CyU3PDebugPrint(4, "ERROR: SensorWrite count > 64\n");
		return 1;
	}
#endif

	/* Set up the I2C control parameters and invoke the write API. */
	preamble.buffer[0] = slaveAddr;
	//preamble.buffer[1] = highAddr;
	//preamble.buffer[2] = lowAddr;
	preamble.buffer[1] = lowAddr;
	//preamble.length = 3;
	preamble.length = 2;
	preamble.ctrlMask = 0x0000;

#if 0
        CyU3PDebugPrint(4, "pre0 %x, pre1%x, pre.len %d, pre.msk %x\r\n",
            preamble.buffer[0],
            preamble.buffer[1],
            preamble.length,
            preamble.ctrlMask);
#endif

	apiRetStatus = CyU3PI2cTransmitBytes(&preamble, &buf, 1, 0);
	
        CyU3PDebugPrint(4, "WR 0x%x, return = %x\r\n", lowAddr, apiRetStatus);
	
	SensorI2CAccessDelay(apiRetStatus);

	return apiRetStatus;
}

CyU3PReturnStatus_t SensorRead2B(uint8_t slaveAddr, uint8_t highAddr,
		uint8_t lowAddr, uint8_t *buf) {
	CyU3PReturnStatus_t apiRetStatus = CY_U3P_SUCCESS;
	CyU3PI2cPreamble_t preamble;

#if 0
	if ((slaveAddr != SENSOR_ADDR_RD) && (slaveAddr != I2C_MEMORY_ADDR_RD)) {
		CyU3PDebugPrint(4, "I2C Slave address is not valid!\n");
		return 1;
	}
#endif

	preamble.buffer[0] = slaveAddr & I2C_SLAVEADDR_MASK; /*  Mask out the transfer type bit. */
	//preamble.buffer[1] = highAddr;
	//preamble.buffer[2] = lowAddr;
	//preamble.buffer[3] = slaveAddr;
	//preamble.length = 4;
	//preamble.ctrlMask = 0x0004; /*  Send start bit after third byte of preamble. */
	preamble.buffer[1] = lowAddr;
	preamble.buffer[2] = slaveAddr;
	preamble.length = 3;
	preamble.ctrlMask = 0x0002; /*  Send start bit after third byte of preamble. */

#if 0
	CyU3PDebugPrint(4, "pre0 %x, pre1 %x, pre2 %x pre.len %d, pre.msk %x\r\n",
            preamble.buffer[0],
            preamble.buffer[1],
            preamble.buffer[2],
            preamble.length,
            preamble.ctrlMask);
#endif

	apiRetStatus = CyU3PI2cReceiveBytes(&preamble, buf, 2, 0);

#if 0
       CyU3PDebugPrint(4, "buf0 %x buf1 %x\r\n",buf[0], buf[1]);
#endif

	CyU3PDebugPrint(4, "RD 0x%x = %x\r\n",lowAddr, apiRetStatus);
	
	SensorI2CAccessDelay(apiRetStatus);

	return apiRetStatus;
}

CyU3PReturnStatus_t SensorRead(uint8_t slaveAddr, uint8_t highAddr,
		uint8_t lowAddr, uint8_t count, uint8_t *buf) {
	CyU3PReturnStatus_t apiRetStatus = CY_U3P_SUCCESS;
	CyU3PI2cPreamble_t preamble;

#if 0
	/* Validate the parameters. */
	if ((slaveAddr != SENSOR_ADDR_RD) && (slaveAddr != I2C_MEMORY_ADDR_RD)) {
		CyU3PDebugPrint(4, "I2C Slave address is not valid!\n");
		return 1;
	}
#endif

	if (count > 64) {
		CyU3PDebugPrint(4, "ERROR: SensorWrite count > 64\n");
		return 1;
	}

	preamble.buffer[0] = slaveAddr & I2C_SLAVEADDR_MASK; /*  Mask out the transfer type bit. */
	//preamble.buffer[1] = highAddr;
	//preamble.buffer[2] = lowAddr;
	//preamble.buffer[3] = slaveAddr;
	//preamble.length = 4;
	//preamble.ctrlMask = 0x0004; /*  Send start bit after third byte of preamble. */
	preamble.buffer[1] = lowAddr;
	preamble.buffer[2] = slaveAddr;
	preamble.length = 3;
	preamble.ctrlMask = 0x0002; /*  Send start bit after third byte of preamble. */

	apiRetStatus = CyU3PI2cReceiveBytes(&preamble, buf, count, 0);
	SensorI2CAccessDelay(apiRetStatus);

	return apiRetStatus;
}

/*
 * Reset the image sensor using GPIO.
 */
void SensorReset(void) {
	CyU3PReturnStatus_t apiRetStatus;

	/* Drive the GPIO low to reset the sensor. */
	apiRetStatus = CyU3PGpioSetValue(SENSOR_RESET_GPIO, CyFalse);
	if (apiRetStatus != CY_U3P_SUCCESS) {
		CyU3PDebugPrint(4, "GPIO Set Value Error, Error Code = %d\n",
				apiRetStatus);
		return;
	}

	/* Wait for some time to allow proper reset. */
	CyU3PThreadSleep(10);

	/* Drive the GPIO high to bring the sensor out of reset. */
	apiRetStatus = CyU3PGpioSetValue(SENSOR_RESET_GPIO, CyTrue);
	if (apiRetStatus != CY_U3P_SUCCESS) {
		CyU3PDebugPrint(4, "GPIO Set Value Error, Error Code = %d\n",
				apiRetStatus);
		return;
	}

	/* Delay the allow the sensor to power up. */
	CyU3PThreadSleep(10);
	return;
}

/* Image sensor initialization sequence. */
void ADV7611Init(void) {

       CyU3PDebugPrint(4, "ADV7611Init\r\n");
       
	if ( ADV7611Write( IO_SlaveAddress_98, 0xFF, 0x80) != CY_U3P_SUCCESS )
	{
	    CyU3PDebugPrint(4, "@: addr %x ADV7611Write failed!\r\n", IO_SlaveAddress_98);
	}

#if 0
	if ( SensorWrite( IO_SlaveAddress_9a, 0x00, 0xFF, 1, buf) != CY_U3P_SUCCESS )
	{
	    CyU3PDebugPrint(4, "@: addr %x SensorWrite failed!\r\n", IO_SlaveAddress_9a);
	}
#endif

	//  I2C reset
       //DelayMS(100) ;
       CyU3PBusyWait (5000);

       //Addr init
       ADV7611Write(IO_SlaveAddress, 0xF4, CEC_SlaveAddress);
       ADV7611Write(IO_SlaveAddress, 0xF5, InfoFrame_SlaveAddress);
       ADV7611Write(IO_SlaveAddress, 0xF8, DPLL_SlaveAddress);
       ADV7611Write(IO_SlaveAddress, 0xF9, KSV_SlaveAddress);
       ADV7611Write(IO_SlaveAddress, 0xFA, EDID_SlaveAddress);
       ADV7611Write(IO_SlaveAddress, 0xFB, HDMI_SlaveAddress);
       ADV7611Write(IO_SlaveAddress, 0xFD, CP_SlaveAddress);

       //

       //  CP
	ADV7611Write( IO_SlaveAddress, 0x01, 0x06);
	// Prim_Mode =110b HDMI-GR
	ADV7611Write( IO_SlaveAddress, 0x02, 0xF5);
	// Auto CSC, RGB out, Set op_656 bit
	ADV7611Write( IO_SlaveAddress, 0x03, 0x80);
	// 24 bit SDR 444 Mode 0
	ADV7611Write( IO_SlaveAddress, 0x05, 0x2C);
	// AV Codes Off
	ADV7611Write( IO_SlaveAddress, 0x06, 0xA7);// bit0:Invert LLC
	// Invert VS,HS pins
	ADV7611Write( IO_SlaveAddress, 0x0B, 0x44);
	// Power up part
	ADV7611Write( IO_SlaveAddress, 0x0C, 0x42);
	// Power up part
	ADV7611Write( IO_SlaveAddress, 0x14, 0x3F);
	// Max Drive Strength

	ADV7611Write( IO_SlaveAddress, 0x15, 0x9E);
	// Disable Tristate of Pins

//	ADV7611Write(pPrivateData, IO_SlaveAddress, 0x15, 0x80);
	// Enable Tristate of Pins


	ADV7611Write( IO_SlaveAddress, 0x19, 0x83);
	// LLC DLL phase
//	ADV7611Write(pPrivateData, IO_SlaveAddress, 0x20, 0x70);
	// 0V Applied to HPA_A pin
	ADV7611Write( IO_SlaveAddress, 0x33, 0x40);
	// LLC DLL enable
	ADV7611Write( CP_SlaveAddress, 0xBA, 0x01);
	// Set HDMI FreeRun
	ADV7611Write( HDMI_SlaveAddress, 0x9B, 0x03);
	// ADI recommended setting
	ADV7611Write( HDMI_SlaveAddress, 0x00, 0x00);
	// Set HDMI Input Port A
	ADV7611Write( HDMI_SlaveAddress, 0x83, 0xFE);
	// Enable clock terminator for port A
	ADV7611Write( HDMI_SlaveAddress, 0x6F, 0x0C);
	// ADI recommended setting
	ADV7611Write( HDMI_SlaveAddress, 0x85, 0x1F);
	// ADI recommended setting
	ADV7611Write( HDMI_SlaveAddress, 0x87, 0x70);
	// ADI recommended setting
	ADV7611Write( HDMI_SlaveAddress, 0x8D, 0x04);
	// LFG
	ADV7611Write( HDMI_SlaveAddress, 0x8E, 0x1E);
	// HFG
	ADV7611Write( HDMI_SlaveAddress, 0x1A, 0x8A);
	// unmute audio
	ADV7611Write( HDMI_SlaveAddress, 0x57, 0xDA);
	// ADI recommended setting
	ADV7611Write( HDMI_SlaveAddress, 0x58, 0x01);
	// ADI recommended setting
	ADV7611Write( HDMI_SlaveAddress, 0x75, 0x10);
	// DDC drive strength
	ADV7611Write( HDMI_SlaveAddress, 0x90, 0x04);
	// LFG
	ADV7611Write( HDMI_SlaveAddress, 0x91, 0x1E);
	// HFG
 	ADV7611Write( HDMI_SlaveAddress, 0x9D, 0x02);
	// ADI Equaliser Setting	

 	ADV7611Write( EDID_SlaveAddress, 0x00, 0x00);
	//Programe EDID

 	ADV7611Write( KSV_SlaveAddress, 0x74, 0x01);
	// Enables I2C access to internal EDID ram from DDC port A

 	ADV7611Write( HDMI_SlaveAddress, 0x6C, 0xA3);
	// HPA takes its value from HPA_MAN_VALUE_A	

	ADV7611Write( IO_SlaveAddress, 0x20, 0x70);
	// 0V Applied to HPA_A pin
	
       //DelayMS(120) ;
       CyU3PBusyWait (120000);
       
	ADV7611Write( IO_SlaveAddress, 0x20, 0xF0);
	// Hi level Applied to HPA_A pin

	//An HDMI Sink shall indicate any change to the contents of the E-EDID by driving a low voltage
	//level pulse on the Hot Plug Detect pin. This pulse shall be at least 100 msec.
	
 	ADV7611Write( HDMI_SlaveAddress, 0x6C, 0xA2);
	// HPA takes its value from HPA_AUTO_INT_EDID	

        //DelayMS(10) ;
        CyU3PBusyWait (10000);
        
       

#if 0	
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xF4, CEC_SlaveAddress);
	//  CEC
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xF5, InfoFrame_SlaveAddress);
	//  INFOFRAME
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xF8, DPLL_SlaveAddress);
	//  DPLL
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xF9, KSV_SlaveAddress);
	// KSV
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xFA, EDID_SlaveAddress);
	// EDID
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xFB, HDMI_SlaveAddress);
	// HDMI
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0xFD, CP_SlaveAddress);
	//  CP
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x01, 0x06);
	// Prim_Mode =110b HDMI-GR
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x02, 0xF5);
	// Auto CSC, RGB out, Set op_656 bit
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x03, 0x80);
	// 24 bit SDR 444 Mode 0
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x05, 0x2C);
	// AV Codes Off
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x06, 0xA7);// bit0:Invert LLC
	// Invert VS,HS pins
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x0B, 0x44);
	// Power up part
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x0C, 0x42);
	// Power up part
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x14, 0x3F);
	// Max Drive Strength

	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x15, 0x9E);
	// Disable Tristate of Pins

//	tmHdmiHalADV7611_SetI2CWriteFunction(pPrivateData, IO_SlaveAddress, 0x15, 0x80);
	// Enable Tristate of Pins


	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x19, 0x83);
	// LLC DLL phase
//	tmHdmiHalADV7611_SetI2CWriteFunction(pPrivateData, IO_SlaveAddress, 0x20, 0x70);
	// 0V Applied to HPA_A pin
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x33, 0x40);
	// LLC DLL enable
	tmHdmiHalADV7611_SetI2CWriteFunction( CP_SlaveAddress, 0xBA, 0x01);
	// Set HDMI FreeRun
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x9B, 0x03);
	// ADI recommended setting
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x00, 0x00);
	// Set HDMI Input Port A
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x83, 0xFE);
	// Enable clock terminator for port A
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x6F, 0x0C);
	// ADI recommended setting
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x85, 0x1F);
	// ADI recommended setting
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x87, 0x70);
	// ADI recommended setting
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x8D, 0x04);
	// LFG
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x8E, 0x1E);
	// HFG
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x1A, 0x8A);
	// unmute audio
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x57, 0xDA);
	// ADI recommended setting
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x58, 0x01);
	// ADI recommended setting
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x75, 0x10);
	// DDC drive strength
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x90, 0x04);
	// LFG
	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x91, 0x1E);
	// HFG
 	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x9D, 0x02);
	// ADI Equaliser Setting	

 	tmHdmiHalADV7611_SetI2CWriteFunction( EDID_SlaveAddress, 0x00, 0x00);
	//Programe EDID

 	tmHdmiHalADV7611_SetI2CWriteFunction( KSV_SlaveAddress, 0x74, 0x01);
	// Enables I2C access to internal EDID ram from DDC port A

 	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x6C, 0xA3);
	// HPA takes its value from HPA_MAN_VALUE_A	

	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x20, 0x70);
	// 0V Applied to HPA_A pin
	
                DelayMS(120) ;
	tmHdmiHalADV7611_SetI2CWriteFunction( IO_SlaveAddress, 0x20, 0xF0);
	// Hi level Applied to HPA_A pin

	//An HDMI Sink shall indicate any change to the contents of the E-EDID by driving a low voltage
	//level pulse on the Hot Plug Detect pin. This pulse shall be at least 100 msec.
	
 	tmHdmiHalADV7611_SetI2CWriteFunction( HDMI_SlaveAddress, 0x6C, 0xA2);
	// HPA takes its value from HPA_AUTO_INT_EDID	


	
        //DelayMS(10) ;
#endif
        
	if (SensorI2cBusTest() != CY_U3P_SUCCESS) /* Verify that the sensor is connected. */
	{
		//CyU3PDebugPrint(4, "Error: Reading Sensor ID failed!\r\n");
		return;
	} else {

	    CyU3PDebugPrint(4, "Test pass, resume adv7611 chk thread\r\n");
	    //CyU3PThreadResume(uvcADV7611LockChk);
	}

	/* Generic settings (which are common for all resolutions) for bringing up the image sensor to stream
	 video data should be populated here.
	 */

	/* Update sensor configuration based on desired video stream parameters. Using 720p 30fps as default setting.*/
	SensorScaling_HD720p_30fps();
}

/*
   Verify that the sensor can be accessed over the I2C bus from FX3.
 */
uint8_t SensorI2cBusTest(void) {
	/* The sensor ID register can be read here to verify sensor connectivity. */
	uint8_t buf[2];

	//CyU3PDebugPrint(4, "addr %x\r\n", SENSOR_ADDR_RD);

	/* Reading sensor ID */
	/*
	if (SensorRead2B(SENSOR_ADDR_RD, 0x00, 0x00, buf) == CY_U3P_SUCCESS) {
		if ((buf[0] == 0x01) && (buf[1] == 0x02)) {
		       
			return CY_U3P_SUCCESS;
		}
	}*/


	if (SensorRead2B(SENSOR_ADDR_RD, 0x00, 0xEA, buf) == CY_U3P_SUCCESS) {
		
		
		CyU3PDebugPrint(4, "OK: 0x%x %x %x\r\n",SENSOR_ADDR_RD, buf[0], buf[1]);

		if ((buf[0] == 0x20) && (buf[1] == 0x51)) {


                    //CyU3PDebugPrint(4, "Test pass\r\n");
                    
			return CY_U3P_SUCCESS;

			
		}
		
	}
	else {
		CyU3PDebugPrint(4, "@: 0x%x %x %x\r\n",SENSOR_ADDR_RD, buf[0], buf[1]);
	}

#if 0
	if (SensorRead(SENSOR_ADDR_RD, 0x00, 0xEA, 1, buf) == CY_U3P_SUCCESS) {
		
		
		CyU3PDebugPrint(4, "OK: 0x%x %x\r\n",SENSOR_ADDR_RD, buf[0]);

		if ((buf[0] == 0x51) && (buf[1] == 0x20)) {

			return CY_U3P_SUCCESS;
		}
		
	}
	else {
		CyU3PDebugPrint(4, "@: 0x%x %x\r\n",SENSOR_ADDR_RD, buf[0]);
	}
#endif



	return 1;
}

/* Function to set the image sensor in VGA streaming mode. */
void SensorScaling_VGA(void) {
/*	Populate particular sensor control commands that will setup the image sensor to stream
	640 * 480 at 15 FPS in this function.
 */
    return;
}

/* Function to set the image sensor in HD720 streaming mode. */
void SensorScaling_HD720p_30fps(void) {
/*	Populate particular sensor control commands that will setup the image sensor to stream
	1280 * 720 at 30 FPS in this function.
 */
    return;
}

/*
 Get the current brightness setting from the image sensor.
 */
uint8_t SensorGetBrightness(void) {
	uint8_t buf[2];

	SensorRead2B(SENSOR_ADDR_RD, 0x00, 0x02, buf);
	return (uint8_t) buf[1];
}

/*
 Update the brightness setting for the image sensor.
 */
void SensorSetBrightness(uint8_t brightness) {
	SensorWrite2B(SENSOR_ADDR_WR, 0x00, 0x02, 0x00, brightness);
}

